/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.techhighteam675.robot2012;

import edu.wpi.first.wpilibj.KinectStick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author TJ
 */
public class AutonomousHandler {

    int autonomousMode;
	
    DriveType driveType;
    RobotDrive robotDrive;
    RobotPayload robotPayload;
	
    SendableChooser modeChooser;
    int[] modesList;
    boolean autonomousStarted;
    Timer autonomousTimer;
    
    public AutonomousHandler(int _mode, RobotDrive _drive, RobotPayload _payload)
    {
	autonomousMode = _mode;
	robotDrive = _drive;
	robotPayload = _payload;
	driveType = null;
	
	autonomousStarted = false;
	autonomousTimer.stop();
	autonomousTimer.reset();
}

void update()
{
	if(!autonomousStarted)
	{
		autonomousTimer.start();
		autonomousStarted = true;
	}
	
	autonomousMode = ((Integer) modeChooser.getSelected()).intValue();
	
	switch(autonomousMode)
	{
	case Definitions.AUTONOMOUS_KINECT:
		// Drive
		if(driveType == null)
		{
                    driveType = new KinectDrive(new KinectStick(0), new KinectStick(1));
                    robotDrive.setDrive(driveType);
		}
		
		// Payload
		robotPayload.setControlMode(RobotPayload.CONTROL_KINECT);
	break;
	
	case Definitions.AUTONOMOUS_STILL:
		// Drive
		if(driveType == null)
		{
			driveType = new AutonomousDrive();
			((AutonomousDrive) driveType).set(0.0F, 0.0F, true);
			robotDrive.setDrive(driveType);
		}
		
		// Payload
		robotPayload.setControlMode(RobotPayload.CONTROL_AUTONOMOUS);
		robotPayload.setShootingMode(RobotPayload.SHOOTING_DISABLED);
		if((autonomousTimer.get() > Definitions.AUTONOMOUS_SHOOT_DELAY 
                        && autonomousTimer.get() < Definitions.AUTONOMOUS_SHOOT_DELAY + Definitions.BELT_SHOOT_TIME))
		{
			robotPayload.setShootingMode(RobotPayload.SHOOTING_AUTOMATIC);
		}
	break;
            
	case Definitions.AUTONOMOUS_FORWARD:
		// Drive
		if(driveType == null)
		{
			driveType = new AutonomousDrive();
			robotDrive.setDrive(driveType);
		}
		
		// Payload
		robotPayload.setControlMode(RobotPayload.CONTROL_AUTONOMOUS);
	break;
	
	case Definitions.AUTONOMOUS_CENTER:
		// Drive
		if(driveType == null)
		{
			driveType = new AutonomousDrive();
			robotDrive.setDrive(driveType);
		}
		
		// Payload
		robotPayload.setControlMode(RobotPayload.CONTROL_AUTONOMOUS);
	break;
	
	default:
	break;
	}
	
	robotDrive.update();
	robotPayload.update();
}

void initChooser()
{
	modeChooser = new SendableChooser();
	modesList = new int[Definitions.AUTONOMOUS_MODES];
	
	modesList[0] = Definitions.AUTONOMOUS_KINECT;
	modesList[1] = Definitions.AUTONOMOUS_STILL;
	modesList[2] = Definitions.AUTONOMOUS_FORWARD;
	modesList[3] = Definitions.AUTONOMOUS_CENTER;
	modesList[4] = Definitions.AUTONOMOUS_DISABLE;
	
	modeChooser.addDefault("Disabled", new Integer(modesList[4]));
	modeChooser.addObject("Kinect Control", new Integer(modesList[0]));
	modeChooser.addObject("Just Shoot", new Integer(modesList[1]));
	modeChooser.addObject("Forward then Shoot", new Integer(modesList[2]));
	modeChooser.addObject("Center Ramp", new Integer(modesList[3]));
	
	SmartDashboard.putData("Autonomous Chooser", modeChooser);
}

}
